#include "pointcloud_tools.h"
#include "point_datagen.h"
#include "point_common.h"
#include <cassert>
#include <cstdio>
#include <stdlib.h>
#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>
#include <vector>
#include <iostream>
#include <fstream>
#include <cinttypes>
#include <filesystem>
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include "ai_common.h"
// namespace fs = std::filesystem;
#define OUTCH_NUM 8
#define INCH_NUM 8
// union Data64to8
// {
// 	uint64_t in64;
// 	int8_t out8[8];
// }wt64to8,in64to8,out64to8;

namespace pointcloud_tools
{
    std::map<int, std::vector<uint32_t>> BlockConfigCache::inst_map;

    Data64to8 wt64to8, in64to8, out64to8;

    void SCONV(LayerConfig layer_cfg,
               BlockConfig block_cfg,
               uint16_t *pt_x,
               uint16_t *pt_y,
               uint16_t *pt_z,
               uint64_t *in_data,
               uint64_t *wt_data,
               uint64_t *out_data)
    {
        int i, j, och, ich, ich_loop;
        int POINT_NUM = block_cfg.cfg_cam_valid_num;
        int inch_loop_num = layer_cfg.cfg_inch_loop_num;
        int CFG_CNN_SIZE_X = layer_cfg.cfg_cnn_size[0];
        int CFG_CNN_SIZE_Y = layer_cfg.cfg_cnn_size[1];
        int CFG_CNN_SIZE_Z = layer_cfg.cfg_cnn_size[2];
        // int WT_DATA_SIZE = 32 * ( layer_cfg.cfg_inch_loop_num + 1);
        int WT_DATA_SIZE = 32 * 4;
        int32_t *out_data_golden_32 = new int32_t[POINT_NUM * OUTCH_NUM];
        int8_t *out_data_golden_8 = new int8_t[POINT_NUM * OUTCH_NUM];
        for (i = 0; i < POINT_NUM; i++)
        {
            for (och = 0; och < OUTCH_NUM; och++)
                out_data_golden_32[i * OUTCH_NUM + och] = 0;
            for (j = 0; j < POINT_NUM; j++)
            {
                int32_t kx = pt_x[j] - pt_x[i] + layer_cfg.cfg_cam_search_range[0];
                int32_t ky = pt_y[j] - pt_y[i] + layer_cfg.cfg_cam_search_range[1];
                int32_t kz = pt_z[j] - pt_z[i] + layer_cfg.cfg_cam_search_range[2];
                if (kx >= 0 && kx <= 2 * layer_cfg.cfg_cam_search_range[0] &&
                    ky >= 0 && ky <= 2 * layer_cfg.cfg_cam_search_range[1] &&
                    kz >= 0 && kz <= 2 * layer_cfg.cfg_cam_search_range[2])
                {
                    for (och = 0; och < OUTCH_NUM; och++)
                    {
                        for (ich_loop = 0; ich_loop < inch_loop_num + 1; ich_loop++)
                        {
                            int wt_index = (kx + ky * CFG_CNN_SIZE_X + kz * CFG_CNN_SIZE_Y * CFG_CNN_SIZE_Z) + ich_loop * CFG_CNN_SIZE_Z * CFG_CNN_SIZE_Y * CFG_CNN_SIZE_X;
                            wt64to8.in64 = wt_data[och * WT_DATA_SIZE + wt_index];
                            in64to8.in64 = in_data[(j + ich_loop * POINT_NUM)];
                            for (ich = 0; ich < INCH_NUM; ich++)
                            {
                                out_data_golden_32[i * OUTCH_NUM + och] += wt64to8.out8[ich] * in64to8.out8[ich];
                            }
                        }
                    }
                }
            }
        }
        for (i = 0; i < POINT_NUM; i++)
        {
            for (och = 0; och < OUTCH_NUM; och++)
            {
                int temp = out_data_golden_32[i * OUTCH_NUM + och] >> layer_cfg.cfg_shift_num;
                if (layer_cfg.cfg_pe_relu_enable && temp < 0)
                {
                    out_data_golden_8[i * OUTCH_NUM + och] = 0;
                }
                else
                {
                    if (temp > 127)
                        out_data_golden_8[i * OUTCH_NUM + och] = 127;
                    else if (temp < -128)
                        out_data_golden_8[i * OUTCH_NUM + och] = -128;
                    else
                        out_data_golden_8[i * OUTCH_NUM + och] = temp;
                }
            }
        }

        for (i = 0; i < POINT_NUM; i++)
        {
            for (och = 0; och < OUTCH_NUM; och++)
            {
                out64to8.out8[och] = out_data_golden_8[i * OUTCH_NUM + och];
            }
            out_data[i] = out64to8.in64;
        }
        delete[] out_data_golden_32;
        delete[] out_data_golden_8;
    }

    void inst_gen_param(
        std::vector<uint32_t> *inst_buf,
        const LayerConfig layer_cfg)
    {
        // inst for CFG_PARAM
        uint16_t cfg_cam_valid_num = 1024;
        uint16_t cfg_total_point_num = 1024;
        uint32_t inst31_0 = ((0x3F & layer_cfg.cfg_start_pt_addr) << 26) | (cfg_total_point_num << 11) | cfg_cam_valid_num;
        uint32_t inst63_32 = (layer_cfg.cfg_cnn_size[1] << 29) | (layer_cfg.cfg_cnn_size[0] << 26) | (layer_cfg.cfg_cam_search_range[2] << 24) | (layer_cfg.cfg_cam_search_range[1] << 22) | (layer_cfg.cfg_cam_search_range[0] << 20) | (layer_cfg.cfg_pe_relu_enable << 19) | (layer_cfg.cfg_shift_num << 11) | (layer_cfg.cfg_inch_loop_num << 9) | ((0x7FC0) & layer_cfg.cfg_start_pt_addr);
        uint32_t inst95_64 = (layer_cfg.cfg_core_enable << 7) | (layer_cfg.cfg_cam_enable << 4) | (layer_cfg.cfg_bias_enable << 3) | (layer_cfg.cfg_cnn_size[2]);
        uint32_t inst127_96 = 4 << 28;

        push_inst(inst_buf, inst31_0, inst63_32, inst95_64, inst127_96);
    }



    void inst_gen_ddr2sram(
        std::vector<uint32_t> *inst_buf,
        LayerConfig layer_cfg,
        BlockConfig block_cfg,
        const char dma_type[32])
    {
        assert(strcmp(dma_type, "wt") == 0 || strcmp(dma_type, "pt") == 0 || strcmp(dma_type, "in") == 0 || strcmp(dma_type, "rs") == 0);
        uint64_t axi_saddr;
        uint32_t sram_saddr;
        int32_t reverved_transfer_num;
        if (strcmp(dma_type, "wt") == 0)
        {
            axi_saddr = layer_cfg.BASE_WT_ADDR;
            sram_saddr = 6 << 17;
            reverved_transfer_num = layer_cfg.DMA_WT_NUM;
        }
        else if (strcmp(dma_type, "pt") == 0)
        {
            axi_saddr = block_cfg.BASE_PT_ADDR;
            sram_saddr = 1 << 17;
            reverved_transfer_num = block_cfg.cfg_total_point_num;
        }
        else if (strcmp(dma_type, "in") == 0)
        {
            axi_saddr = block_cfg.BASE_IN_ADDR;
            sram_saddr = 4 << 17;
            reverved_transfer_num = (layer_cfg.cfg_inch_loop_num + 1) * block_cfg.cfg_total_point_num;
        }
        else
        {
            exit(1);
        }
        int transfer_num;
        // bool cross_4k_boundary = 0;
        // if(reverved_transfer_num > 512){
        //     cross_4k_boundary = ((axi_saddr & ~0xFFFULL) != axi_saddr) ;
        // } else {
        //     uint64_t axi_eaddr = axi_saddr + reverved_transfer_num * 8;
        //     cross_4k_boundary = ((axi_saddr & ~0xFFFULL) != (axi_eaddr & ~0xFFFULL)) ;
        // }
        // if(cross_4k_boundary){
        //     printf("cross_4k_boundary: axi_saddr = 0x%lx, sram_saddr = 0x%x, reverved_transfer_num = %d\n", axi_saddr, sram_saddr, reverved_transfer_num);
        //     transfer_num = ( 0x1000 - (axi_saddr & 0xFFFULL)) / 8;
        //     uint32_t inst31_0 =  (( axi_saddr & 0x1FFFF )<< 15)| transfer_num; //{axi_saddr[16:0] , cfg_total_pt_num[14:0]}
        //     uint32_t inst63_32 = ((sram_saddr & 0x1FF)<<23) | (axi_saddr>>17); //{sram_saddr[8:0],axi_saddr[39:17] 23}
        //     uint32_t inst95_64 = (1<<11) |  (sram_saddr>>9) ;//{sram_saddr[19:9]}
        //     uint32_t inst127_96 = 5<<28;

        //     inst_buf->push_back(inst31_0);
        //     inst_buf->push_back(inst63_32);
        //     inst_buf->push_back(inst95_64);
        //     inst_buf->push_back(inst127_96);

        //     sram_saddr = sram_saddr + transfer_num;
        //     printf("axi_saddr begin: %x\n", axi_saddr);
        //     axi_saddr = axi_saddr + transfer_num*8;
        //     reverved_transfer_num = reverved_transfer_num - transfer_num;
        //     printf("transfer_num: %d \n", transfer_num);
        //     printf("axi_saddr next: 0x%x\n", axi_saddr);
        //     printf("reverved_transfer_num: %d\n", reverved_transfer_num);
        // }

        while (reverved_transfer_num > 0)
        {
            if (reverved_transfer_num > 512)
            {
                transfer_num = 512;
            }
            else
            {
                transfer_num = reverved_transfer_num;
            }
            uint32_t inst31_0 = ((axi_saddr & 0x1FFFF) << 15) | transfer_num;      //{axi_saddr[16:0] , cfg_total_pt_num[14:0]}
            uint32_t inst63_32 = ((sram_saddr & 0x1FF) << 23) | (axi_saddr >> 17); //{sram_saddr[8:0],axi_saddr[39:17] 23}
            uint32_t inst95_64 = (1 << 11) | (sram_saddr >> 9);                    //{sram_saddr[19:9]}
            uint32_t inst127_96 = 5 << 28;

            push_inst(inst_buf, inst31_0, inst63_32, inst95_64, inst127_96);

            sram_saddr = sram_saddr + transfer_num;
            axi_saddr = axi_saddr + transfer_num * 8;
            reverved_transfer_num = reverved_transfer_num - 512;
        }
    }


    void inst_gen_sram2ddr(
        std::vector<uint32_t> *inst_buf,
        LayerConfig layer_cfg,
        BlockConfig block_cfg)
    {
        uint64_t axi_saddr;
        uint32_t sram_saddr;
        int32_t reverved_transfer_num;

        int used_core_num = get_used_core_num(layer_cfg.cfg_core_enable);

        for (int i = 0; i < used_core_num; i++)
        { // core0 - core 3
            sram_saddr = (5 << 17) | (i << 15);
            axi_saddr = block_cfg.BASE_RS_ADDR + (0x2000 * i);

            reverved_transfer_num = block_cfg.cfg_total_point_num;
            // printf("RS: axi_saddr = 0x%lx, sram_saddr = 0x%x, transfer_num = %d \n", axi_saddr, sram_saddr,reverved_transfer_num);
            int transfer_num;
            while (reverved_transfer_num > 0)
            {
                if (reverved_transfer_num > 512)
                {
                    transfer_num = 512;
                }
                else
                {
                    transfer_num = reverved_transfer_num;
                }
                uint32_t inst31_0 = ((axi_saddr & 0x1FFFF) << 15) | transfer_num;      //{axi_saddr[16:0] , cfg_total_pt_num[14:0]}
                uint32_t inst63_32 = ((sram_saddr & 0x1FF) << 23) | (axi_saddr >> 17); //{sram_saddr[8:0],axi_saddr[39:17] 23}
                uint32_t inst95_64 = (0 << 11) | (sram_saddr >> 9);                    //{sram_saddr[19:9]}
                uint32_t inst127_96 = 5 << 28;

                push_inst(inst_buf, inst31_0, inst63_32, inst95_64, inst127_96);

                sram_saddr = sram_saddr + 0x200;
                axi_saddr = axi_saddr + 0x1000;
                reverved_transfer_num = reverved_transfer_num - 512;
            }
        }
    }

    void inst_gen_bias(
        std::vector<uint32_t> *inst_buf,
        LayerConfig layer_cfg)
    {
        uint32_t inst31_0 = (layer_cfg.bias_core0[3] << 8) | (layer_cfg.bias_core0[2] << 8) | (layer_cfg.bias_core0[1] << 8) | layer_cfg.bias_core0[0];
        uint32_t inst63_32 = (layer_cfg.bias_core0[7] << 8) | (layer_cfg.bias_core0[6] << 8) | (layer_cfg.bias_core0[5] << 8) | layer_cfg.bias_core0[4];
        uint32_t inst95_64 = 0;
        uint32_t inst127_96 = 0;

        push_inst(inst_buf, inst31_0, inst63_32, inst95_64, inst127_96);

        inst31_0 = (layer_cfg.bias_core1[3] << 8) | (layer_cfg.bias_core1[2] << 8) | (layer_cfg.bias_core1[1] << 8) | layer_cfg.bias_core1[0];
        inst63_32 = (layer_cfg.bias_core1[7] << 8) | (layer_cfg.bias_core1[6] << 8) | (layer_cfg.bias_core1[5] << 8) | layer_cfg.bias_core1[4];
        inst95_64 = 0;
        inst127_96 = 0;

        push_inst(inst_buf, inst31_0, inst63_32, inst95_64, inst127_96);

        inst31_0 = (layer_cfg.bias_core2[3] << 8) | (layer_cfg.bias_core2[2] << 8) | (layer_cfg.bias_core2[1] << 8) | layer_cfg.bias_core2[0];
        inst63_32 = (layer_cfg.bias_core2[7] << 8) | (layer_cfg.bias_core2[6] << 8) | (layer_cfg.bias_core2[5] << 8) | layer_cfg.bias_core2[4];
        inst95_64 = 0;
        inst127_96 = 0;

        push_inst(inst_buf, inst31_0, inst63_32, inst95_64, inst127_96);

        inst31_0 = (layer_cfg.bias_core3[3] << 8) | (layer_cfg.bias_core3[2] << 8) | (layer_cfg.bias_core3[1] << 8) | layer_cfg.bias_core3[0];
        inst63_32 = (layer_cfg.bias_core3[7] << 8) | (layer_cfg.bias_core3[6] << 8) | (layer_cfg.bias_core3[5] << 8) | layer_cfg.bias_core3[4];
        inst95_64 = 0;
        inst127_96 = 0;

        push_inst(inst_buf, inst31_0, inst63_32, inst95_64, inst127_96);
    }

    void inst_gen_cnn(
        std::vector<uint32_t> *inst_buf,
        LayerConfig layer_cfg,
        BlockConfig block_cfg)
    {
        uint32_t inst31_0 = ((0x3F & layer_cfg.cfg_start_pt_addr) << 26) | (block_cfg.cfg_total_point_num << 11) | block_cfg.cfg_cam_valid_num; // {cfg_start_pt_addr[5:0], cfg_total_pt_num[14:0], cfg_cam_valid_num[10:0]}
        uint32_t inst63_32 = layer_cfg.cfg_start_pt_addr >> 6;                                                                                  // cfg_start_pt_addr[14:6]
        uint32_t inst95_64 = 0;
        uint32_t inst127_96 = 7 << 28;

        push_inst(inst_buf, inst31_0, inst63_32, inst95_64, inst127_96);
    }


    void inst_gen_finish(
        std::vector<uint32_t> *inst_buf)
    {
        uint32_t inst31_0 = 0;
        uint32_t inst63_32 = 0;
        uint32_t inst95_64 = 0;
        uint32_t inst127_96 = 9 << 28;

        push_inst(inst_buf, inst31_0, inst63_32, inst95_64, inst127_96);
    }

    void Pointcloud_utils::wt_init()
    {
        // ///////////////////////////////////////////////////////////////////
        // //    Generate the weight instruction and direcly dump to file   //
        // ///////////////////////////////////////////////////////////////////
        // std::vector<uint32_t> _wt_inst;
        // // inst_gen_param(&_wt_inst,Pointcloud_utils::layer_config);
        // BlockConfig block_cfg;
        // // Unused params, just for init
        // block_cfg.cfg_cam_valid_num = 1024;
        // block_cfg.cfg_total_point_num = 1024;
        // block_cfg.BASE_PT_ADDR = 0;
        // block_cfg.BASE_IN_ADDR = 0;
        // block_cfg.BASE_RS_ADDR = 0;

        // inst_gen_bias(
        //     &_wt_inst,
        //     Pointcloud_utils::layer_config
        // );
        // inst_gen_ddr2sram(
        //     &_wt_inst,
        //     Pointcloud_utils::layer_config,
        //     block_cfg,
        //     "wt"
        // );

        // inst_gen_finish(&_wt_inst);
        // // Pointcloud_utils::wt_inst  = _wt_inst;
        // printf("Pointcloud_utils::wt_inst_dump: Dump inst to folder %s \n", Pointcloud_utils::save_folder);
        // check_folder(Pointcloud_utils::save_folder);
        // char _save_path[256];
        // // sprintf(_save_path, "%s/inst_weight.bin", save_folder);
        // sprintf(_save_path, "%s/inst_weight_%d.bin", Pointcloud_utils::save_folder, Pointcloud_utils::layer_config.layer_id);
        // bool ret;
        // ret = exportVecToBin(_wt_inst, _save_path);
        // if(!ret){
        //     printf("Error: Pointcloud_utils::wt_inst");
        // }
        ///////////////////////////////////////////////////////////////////
        //                  Initialize the weight in DDR                 //
        ///////////////////////////////////////////////////////////////////

        uint64_t *DDRdata = nullptr;
        LayerConfig layer_cfg = Pointcloud_utils::layer_config;
        if (Pointcloud_utils::mappedDDR != MAP_FAILED)
        {
            DDRdata = static_cast<uint64_t *>(Pointcloud_utils::mappedDDR);
        }
        // char _load_path[256];
        // sprintf(_load_path, "%s/weight.bin", save_folder);

        // Read WT
        assert(layer_cfg.DMA_WT_NUM > 0 && layer_cfg.DMA_WT_NUM <= 32 * 4 * 8 * 4);

        // uint64_t* wt_data = new uint64_t[layer_cfg.DMA_WT_NUM];

        // assert(read_bin_file(_load_path, wt_data, layer_cfg.DMA_WT_NUM));
        assert((layer_cfg.BASE_WT_ADDR - Pointcloud_utils::DDR_BASE_ADDR) % 8 == 0);
        uint64_t wt_offset = (layer_cfg.BASE_WT_ADDR - Pointcloud_utils::DDR_BASE_ADDR) / 8;
        if(DDRdata){
            memcpy(DDRdata + wt_offset, layer_cfg.wt_data.data(), layer_cfg.wt_data.size() * sizeof(uint64_t));
    
        }
    }
    void Pointcloud_utils::compute_init()
    {
        // ///////////////////////////////////////////////////////////////////
        // //   Generate the compute instruction and direcly dump to file   //
        // ///////////////////////////////////////////////////////////////////
        // std::vector<uint32_t> _compute_inst;
        // inst_gen_param(
        //     &_compute_inst,eed3
        //     Pointcloud_utils::layer_config
        // );
        // for (size_t i = 0; i < Pointcloud_utils::block_config.size(); i++){
        //     BlockConfig cfg = Pointcloud_utils::block_config[i];

        //     inst_gen_ddr2sram(
        //         &_compute_inst,
        //         Pointcloud_utils::layer_config,
        //         cfg,
        //         "pt"
        //     );

        //     inst_gen_ddr2sram(
        //         &_compute_inst,
        //         Pointcloud_utils::layer_config,
        //         cfg,
        //         "in"
        //     );

        //     inst_gen_cnn(
        //         &_compute_inst,
        //         Pointcloud_utils::layer_config,
        //         cfg
        //     );

        //     inst_gen_sram2ddr(
        //         &_compute_inst,
        //         Pointcloud_utils::layer_config,
        //         cfg
        //     );
        // }
        // inst_gen_finish(&_compute_inst);
        // // Pointcloud_utils::compute_inst  = _compute_inst;
        // printf("Pointcloud_utils::compute_inst_dump: Dump inst to folder %s \n", Pointcloud_utils::save_folder);
        // check_folder(save_folder);
        // char _save_path[256];
        // // sprintf(_save_path, "%s/inst_compute.bin", save_folder);
        // sprintf(_save_path, "%s/inst_compute_%d.bin", Pointcloud_utils::save_folder, Pointcloud_utils::layer_config.layer_id);
        // bool ret;
        // ret = exportVecToBin(_compute_inst, _save_path);
        // if(!ret){
        //     printf("Error: Pointcloud_utils::compute_inst");
        // }
        ///////////////////////////////////////////////////////////////////
        //               Initialize the Pt & Feat in DDR                 //
        ///////////////////////////////////////////////////////////////////

        uint64_t *DDRdata = nullptr;
        LayerConfig layer_cfg = Pointcloud_utils::layer_config;
        if (Pointcloud_utils::mappedDDR != MAP_FAILED)
        {
            DDRdata = static_cast<uint64_t *>(Pointcloud_utils::mappedDDR);
        }

        for (int i = 0; i < int(Pointcloud_utils::block_config.size()); i++)
        {
            BlockConfig block_cfg = Pointcloud_utils::block_config[i];
            // Read Point
            assert(block_cfg.pt_data.size() == size_t(block_cfg.cfg_cam_valid_num));
            assert((block_cfg.BASE_PT_ADDR - Pointcloud_utils::DDR_BASE_ADDR) % 8 == 0);
            uint64_t pt_offset = (block_cfg.BASE_PT_ADDR - Pointcloud_utils::DDR_BASE_ADDR) / 8;
            memcpy(DDRdata + pt_offset, block_cfg.pt_data.data(), block_cfg.cfg_cam_valid_num * sizeof(uint64_t));

            // Read IN
            assert(block_cfg.in_data.size() == size_t(block_cfg.cfg_cam_valid_num * (layer_cfg.cfg_inch_loop_num + 1)));
            assert((block_cfg.BASE_IN_ADDR - Pointcloud_utils::DDR_BASE_ADDR) % 8 == 0);
            uint64_t in_offset = (block_cfg.BASE_IN_ADDR - Pointcloud_utils::DDR_BASE_ADDR) / 8;
            memcpy(DDRdata + in_offset, block_cfg.in_data.data(), block_cfg.cfg_cam_valid_num * (layer_cfg.cfg_inch_loop_num + 1) * sizeof(uint64_t));
        }
    }

    bool Pointcloud_utils::Model_DDR_alloc(const char *filename)
    {
        ddrSize = Pointcloud_utils::ddrSize;

        // 创建一个新文件
        Pointcloud_utils::ddr_fd = open(filename, O_RDWR | O_CREAT | O_TRUNC, 0666);
        if (Pointcloud_utils::ddr_fd == -1)
        {
            std::cerr << "Unable to open ddr file" << std::endl;
            return false;
        }

        // 将文件大小扩展到指定大小
        if (ftruncate(Pointcloud_utils::ddr_fd, ddrSize) == -1)
        {
            std::cerr << "Unable to extend ddr file" << std::endl;
            close(Pointcloud_utils::ddr_fd);
            return false;
        }

        // 进行内存映射
        Pointcloud_utils::mappedDDR = mmap(nullptr, ddrSize, PROT_READ | PROT_WRITE, MAP_SHARED, Pointcloud_utils::ddr_fd, 0);
        if (mappedDDR == MAP_FAILED)
        {
            std::cerr << "mmap failed for mappedDDR" << std::endl;
            close(Pointcloud_utils::ddr_fd);
            return false;
        }

        return true;
    }

    bool Pointcloud_utils::Model_DDR_close()
    {
        if (Pointcloud_utils::mappedDDR != MAP_FAILED)
        {
            if (munmap(Pointcloud_utils::mappedDDR, Pointcloud_utils::ddrSize) == -1)
            {
                std::cerr << "Failed to close mappedDDR file" << std::endl;
                return false;
            }
        }
        if (Pointcloud_utils::ddr_fd != -1)
        {
            close(Pointcloud_utils::ddr_fd);
            return true;
        }
        return false;
    }

    int Pointcloud_utils::inst_gen_block(BlockConfig cfg)
    {
        std::vector<uint32_t> _compute_inst;
        int cnn_inst_index = 0;

        inst_gen_param(
            &_compute_inst,
            Pointcloud_utils::layer_config);

        inst_gen_ddr2sram(
            &_compute_inst,
            Pointcloud_utils::layer_config,
            cfg,
            "pt");

        inst_gen_ddr2sram(
            &_compute_inst,
            Pointcloud_utils::layer_config,
            cfg,
            "in");

        cnn_inst_index = _compute_inst.size(); // 4 is the number of uint32_t in each instruction
        
        // printf("cnn_inst_%d\n", cnn_inst_index);

        inst_gen_cnn(
            &_compute_inst,
            Pointcloud_utils::layer_config,
            cfg);

        inst_gen_sram2ddr(
            &_compute_inst,
            Pointcloud_utils::layer_config,
            cfg);

        inst_gen_finish(&_compute_inst);

        // memcpy(dma_inst, _compute_inst.data(), _compute_inst.size() * sizeof(uint32_t));

        return cnn_inst_index; // Return the index of the CNN instruction in the compute instruction
    }

    void Pointcloud_utils::run()
    {
        uint64_t *DDRdata = nullptr;
        LayerConfig layer_cfg = Pointcloud_utils::layer_config;
        if (Pointcloud_utils::mappedDDR != MAP_FAILED)
        {
            DDRdata = static_cast<uint64_t *>(mappedDDR);
        }

        // // Read WT
        assert(layer_cfg.DMA_WT_NUM > 0 && layer_cfg.DMA_WT_NUM <= 32 * 4 * 8 * 4);

        uint64_t wt_offset = (layer_cfg.BASE_WT_ADDR - Pointcloud_utils::DDR_BASE_ADDR) / 8;

        // uint64_t* wt_data = new uint64_t[layer_cfg.DMA_WT_NUM];

        // assert((layer_cfg.BASE_WT_ADDR - Pointcloud_utils::DDR_BASE_ADDR) % 8 == 0);
        // uint64_t wt_offset = (layer_cfg.BASE_WT_ADDR - Pointcloud_utils::DDR_BASE_ADDR) / 8;
        // memcpy(wt_data, DDRdata + wt_offset, layer_cfg.DMA_WT_NUM * sizeof(uint64_t));

        // Block Computing
        for (int i = 0; i < int(Pointcloud_utils::block_config.size()); i++)
        {
            BlockConfig block_cfg = Pointcloud_utils::block_config[i];
            // Read PT
            uint64_t *pt_data = new uint64_t[block_cfg.cfg_cam_valid_num];
            uint16_t *pt_x = new uint16_t[block_cfg.cfg_cam_valid_num];
            uint16_t *pt_y = new uint16_t[block_cfg.cfg_cam_valid_num];
            uint16_t *pt_z = new uint16_t[block_cfg.cfg_cam_valid_num];
            assert((block_cfg.BASE_PT_ADDR - Pointcloud_utils::DDR_BASE_ADDR) % 8 == 0);
            uint64_t pt_offset = (block_cfg.BASE_PT_ADDR - Pointcloud_utils::DDR_BASE_ADDR) / 8;
            memcpy(pt_data, DDRdata + pt_offset, block_cfg.cfg_cam_valid_num * sizeof(uint64_t));
            for (int p = 0; p < block_cfg.cfg_cam_valid_num; p++)
            {
                pt_x[p] = pt_data[p] & 0xfff;
                pt_y[p] = (pt_data[p] & 0xfff000) >> 12;
                pt_z[p] = (pt_data[p] & 0xfff000000) >> 24;
            }
            delete[] pt_data;

            // Conv Compute
            int used_core_num = get_used_core_num(layer_cfg.cfg_core_enable);

            Pointcloud_utils::block_config[i].rs_data.resize(block_cfg.cfg_cam_valid_num * used_core_num);

            // Read IN
            uint64_t *in_data = new uint64_t[block_cfg.cfg_cam_valid_num * (layer_cfg.cfg_inch_loop_num + 1)];
            assert((block_cfg.BASE_IN_ADDR - Pointcloud_utils::DDR_BASE_ADDR) % 8 == 0);
            uint64_t in_offset = ((block_cfg.BASE_IN_ADDR - Pointcloud_utils::DDR_BASE_ADDR) / 8);
            memcpy(in_data, DDRdata + in_offset, block_cfg.cfg_cam_valid_num * (layer_cfg.cfg_inch_loop_num + 1) * sizeof(uint64_t));

            int weight_per_core = OUTCH_NUM * 4 * 32;
            // int weight_per_core = OUTCH_NUM * (layer_cfg.cfg_inch_loop_num + 1) * 32;
            for (int core = 0; core < used_core_num; core++)
            {
                // Read WT
                uint64_t *wt_data = new uint64_t[weight_per_core];
                uint64_t core_wt_offset = wt_offset + core * weight_per_core;
                memcpy(wt_data, DDRdata + core_wt_offset, weight_per_core * sizeof(uint64_t));

                uint64_t *out_data = new uint64_t[block_cfg.cfg_cam_valid_num];
                SCONV(
                    layer_cfg,
                    block_cfg,
                    pt_x,
                    pt_y,
                    pt_z,
                    in_data,
                    wt_data,
                    out_data
                    // core
                );
                // RS Write
                assert((block_cfg.BASE_RS_ADDR - Pointcloud_utils::DDR_BASE_ADDR + (0x2000 * core)) % 8 == 0);
                uint64_t rs_offset = (block_cfg.BASE_RS_ADDR - Pointcloud_utils::DDR_BASE_ADDR + (0x2000 * core)) / 8;
                memcpy(DDRdata + rs_offset, out_data, block_cfg.cfg_cam_valid_num * sizeof(uint64_t));
                // Pointcloud_utils::block_config[i].rs_data.clear();
                // Pointcloud_utils::block_config[i].rs_data.resize(block_cfg.cfg_cam_valid_num);
                // memcpy(Pointcloud_utils::block_config[i].rs_data.data(), out_data, block_cfg.cfg_cam_valid_num * sizeof(uint64_t)) ;

                // Write each block
                uint64_t *target_ptr = Pointcloud_utils::block_config[i].rs_data.data() + core * block_cfg.cfg_cam_valid_num;
                memcpy(target_ptr, out_data, block_cfg.cfg_cam_valid_num * sizeof(uint64_t));

                // printf("Copy Len: %d\n", block_cfg.cfg_cam_valid_num * sizeof(uint64_t));
                char _save_path[256];
                sprintf(_save_path, "%s/result_block_%06d_pointnum_%07d_core_%d_layer_%d.bin", Pointcloud_utils::save_folder, i, block_cfg.cfg_cam_valid_num, core, layer_cfg.layer_id);
                check_folder(Pointcloud_utils::save_folder);
                write_bin_file(_save_path, out_data, block_cfg.cfg_cam_valid_num);
                delete[] out_data;
                delete[] wt_data;
            }
            delete[] in_data;
            delete[] pt_x;
            delete[] pt_y;
            delete[] pt_z;
        }

        // delete[] wt_data;
    }

    void Pointcloud_utils::Model_RandomDataGenerator()
    {
        printf("Pointcloud_utils::Model_RandomDataGenerator: Generate in floder %s \n", save_folder);
        check_folder(save_folder);
        char _save_path[256];
        sprintf(_save_path, "%s/weight.bin", save_folder);
        wt_rand_generator(layer_config.DMA_WT_NUM, _save_path);
        for (int i = 0; i < int(Pointcloud_utils::block_config.size()); i++)
        {
            BlockConfig block_cfg = Pointcloud_utils::block_config[i];
            sprintf(_save_path, "%s/point_block_%06d.bin", save_folder, i);
            pt_rand_generator(block_cfg.cfg_cam_valid_num, _save_path);
            sprintf(_save_path, "%s/input_block_%06d.bin", save_folder, i);
            in_rand_generator(block_cfg.cfg_cam_valid_num, layer_config.cfg_inch_loop_num, _save_path);
        }
    }

    /////////////////////////////////////////////////////////////////////////
    ///////////////////////   POINTCLOUD ONLINE UTILS   /////////////////////
    /////////////////////////////////////////////////////////////////////////

    static int open_checked(const char *name, int flags)
    {
        int fd = open(name, flags);
        if (fd == -1)
        {
            printf("[error] when open `%s`: %s\n", name, strerror(errno));
        }
        return fd;
    }

    void *pmem_mmap(int devmem_fd, size_t base, size_t length)
    {
        // long page_size = sysconf(_SC_PAGESIZE);
        // assert(base % page_size == 0); // 页对齐检查
        assert(base % sysconf(_SC_PAGESIZE) == 0);
        void *ptr = mmap(
            NULL,
            length,
            PROT_READ | PROT_WRITE,
            MAP_SHARED,
            devmem_fd,
            base);
        if (ptr == MAP_FAILED)
        {
            printf("[error] mmap /dev/mem at %p: %s\n", (void *)base, strerror(errno));
            exit(1);
        }
        return ptr;
    }

#define SRAM_BASE 0x640000000ULL
#define SRAM_SIZE (16 * 1024)
#define DDR_BASE 0x95000000ULL
#define DDR_SIZE (256 * 1024 * 1024)
#define APB_BASE 0x37000000ULL

    void Pointcloud_online_utils::pmem_mmap_init()
    {
        devmem_fd = open_checked("/dev/mem", O_SYNC | O_RDWR);
        dma_inst = (uint32_t *)pmem_mmap(devmem_fd, SRAM_BASE, SRAM_SIZE);
        ddrbuf = (uint64_t *)pmem_mmap(devmem_fd, DDR_BASE, DDR_SIZE);
        apb_regs = (apb_regs_block *)pmem_mmap(devmem_fd, APB_BASE, sizeof(apb_regs_block));
        // 其他寄存器空间可按需映射
        apb_regs->TOP_CHOOSE = 0x2;
    }

    void Pointcloud_online_utils::sram_rw_test()
    {
        printf("[SRAM] 写入测试数据...\n");
        for (size_t i = 0; i < SRAM_SIZE / 4; ++i)
        {
            dma_inst[i] = (uint32_t)(0xA5A50000 | (i & 0xFFFF));
            usleep(1);
        }
        printf("[SRAM] 读取并校验...\n");
        for (size_t i = 0; i < SRAM_SIZE / 4; ++i)
        {
            uint32_t val = dma_inst[i];
            uint32_t expect = (uint32_t)(0xA5A50000 | (i & 0xFFFF));
            if (val != expect)
            {
                printf("[SRAM] 校验失败: idx=%zu, val=0x%08x, expect=0x%08x\n", i, val, expect);
                return;
            }
        }
        printf("[SRAM] 读写测试通过！\n");
    }

    void Pointcloud_online_utils::ddr_rw_test()
    {
        printf("[DDR] 写入测试数据...\n");
        for (size_t i = 0; i < (1024 * 1024) / 4; ++i)
        {
            ddrbuf[i] = (uint32_t)(0x5A5A0000 | (i & 0xFFFF));
        }
        printf("[DDR] 读取并校验...\n");
        for (size_t i = 0; i < (1024 * 1024) / 4; ++i)
        {
            uint32_t val = ddrbuf[i];
            uint32_t expect = (uint32_t)(0x5A5A0000 | (i & 0xFFFF));
            if (val != expect)
            {
                printf("[DDR] 校验失败: idx=%zu, val=0x%08x, expect=0x%08x\n", i, val, expect);
                return;
            }
        }
        printf("[DDR] 读写测试通过！\n");
    }

    void Pointcloud_online_utils::wt_init()
    {
        // For online init, we only need to configure the Pointcloud_online_utils::layer_config.wt_data
        std::vector<uint32_t> _wt_inst;
        BlockConfig block_cfg;
        LayerConfig layer_cfg = Pointcloud_online_utils::layer_config;
        // Unused params, just for init
        block_cfg.cfg_cam_valid_num = 1024;
        block_cfg.cfg_total_point_num = 1024;
        block_cfg.BASE_PT_ADDR = 0;
        block_cfg.BASE_IN_ADDR = 0;
        block_cfg.BASE_RS_ADDR = 0;
        inst_gen_bias(
            &_wt_inst,
            Pointcloud_online_utils::layer_config);
        inst_gen_ddr2sram(
            &_wt_inst,
            Pointcloud_online_utils::layer_config,
            block_cfg,
            "wt");
        inst_gen_finish(&_wt_inst);
        // Pointcloud_utils::wt_inst  = _wt_inst;
        // Directly write inst to the SRAM
        memcpy(const_cast<uint32_t*>(dma_inst), _wt_inst.data(), _wt_inst.size() * sizeof(uint32_t));

        // Write weight to DDR from layer_cfg.wt_data
        uint64_t wt_offset = (layer_cfg.BASE_WT_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) / 8;
        memcpy(const_cast<uint64_t*>(ddrbuf) + wt_offset, layer_cfg.wt_data.data(), layer_cfg.wt_data.size() * sizeof(uint64_t));

        // printf("COPY SIZE: %d \n", _wt_inst.size());
        // printf("WT_INST[0]= %X \n", _wt_inst[0]);
        // printf("DMA INST[0] = %X \n", dma_inst[0]);
        // printf("Weight inst generated and written to SRAM.\n");
        // Start
        apb_regs->CFG_FINISH_CLR = 1;
        apb_regs->CFG_FINISH_CLR = 0;
        apb_regs->TOP_START = 0;
        apb_regs->TOP_START = 1;
        apb_regs->TOP_START = 0;
        printf("Wait for Finish \n");
        while (apb_regs->CFG_TOP_FINISH == 0)
            ;
        printf("Write weight successfully. \n");
        apb_regs->CFG_FINISH_CLR = 1;
        apb_regs->CFG_FINISH_CLR = 0;
    }

    void Pointcloud_online_utils::wt_init(const char inst_file[256], const char weight_file[256])
    {
        // For offline init,  we need to generate the weight and inst files before running this function.
        LayerConfig layer_cfg = Pointcloud_online_utils::layer_config;
        uint64_t wt_offset = (layer_cfg.BASE_WT_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) / 8;
        read_bin_file(weight_file, const_cast<uint64_t*>(ddrbuf) + wt_offset);

        read_bin_file(inst_file, const_cast<uint32_t*>(dma_inst));


        // Start
        apb_regs->CFG_FINISH_CLR = 1;
        apb_regs->CFG_FINISH_CLR = 0;
        apb_regs->TOP_START = 0;
        apb_regs->TOP_START = 1;
        apb_regs->TOP_START = 0;
        while (apb_regs->CFG_TOP_FINISH == 0)
            ;
        printf("Write weight successfully. \n");
        apb_regs->CFG_FINISH_CLR = 1;
        apb_regs->CFG_FINISH_CLR = 0;
    }

    void Pointcloud_online_utils::compute_init()
    {
        // We always need online generator because we achieve the partition algorithm online.
        // For online generator, we need to configure the Pointcloud_online_utils::block_config and Pointcloud_online_utils::layer_config.
        std::vector<uint32_t> _compute_inst;

        for (size_t i = 0; i < Pointcloud_online_utils::block_config.size(); i++)
        {
            BlockConfig cfg = Pointcloud_online_utils::block_config[i];
            inst_gen_param(
                &_compute_inst,
                Pointcloud_online_utils::layer_config);

            inst_gen_ddr2sram(
                &_compute_inst,
                Pointcloud_online_utils::layer_config,
                cfg,
                "pt");

            inst_gen_ddr2sram(
                &_compute_inst,
                Pointcloud_online_utils::layer_config,
                cfg,
                "in");

            inst_gen_cnn(
                &_compute_inst,
                Pointcloud_online_utils::layer_config,
                cfg);

            inst_gen_sram2ddr(
                &_compute_inst,
                Pointcloud_online_utils::layer_config,
                cfg);
        }
        inst_gen_finish(&_compute_inst);
        // Directly write inst to the SRAM
        memcpy(const_cast<uint32_t*>(dma_inst), _compute_inst.data(), _compute_inst.size() * sizeof(uint32_t)); // Note that we've already made use of the wt_inst in the SRAM, so we can directly write the compute inst to the SAME ADDR in the SRAM.

        // Write the Point and Feature data to DDR
        for (int i = 0; i < int(Pointcloud_online_utils::block_config.size()); i++)
        {
            BlockConfig block_cfg = Pointcloud_online_utils::block_config[i];
            // Read Point
            assert(block_cfg.pt_data.size() == size_t(block_cfg.cfg_cam_valid_num));
            assert((block_cfg.BASE_PT_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) % 8 == 0);
            uint64_t pt_offset = (block_cfg.BASE_PT_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) / 8;
            memcpy(const_cast<uint64_t*>(ddrbuf) + pt_offset, block_cfg.pt_data.data(), block_cfg.cfg_cam_valid_num * sizeof(uint64_t));

            // Read IN
            assert(block_cfg.in_data.size() == size_t(block_cfg.cfg_cam_valid_num * (Pointcloud_online_utils::layer_config.cfg_inch_loop_num + 1)));
            assert((block_cfg.BASE_IN_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) % 8 == 0);
            uint64_t in_offset = (block_cfg.BASE_IN_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) / 8;
            memcpy(const_cast<uint64_t*>(ddrbuf) + in_offset, block_cfg.in_data.data(), block_cfg.cfg_cam_valid_num * (Pointcloud_online_utils::layer_config.cfg_inch_loop_num + 1) * sizeof(uint64_t));
        }
    }

    // Instruction generation for blocks
    // The only instruction need to be generated is the CNN instruction
    int Pointcloud_online_utils::inst_gen_block(BlockConfig cfg)
    {
        std::vector<uint32_t> _compute_inst;
        int cnn_inst_index = 0;

        inst_gen_param(
            &_compute_inst,
            Pointcloud_online_utils::layer_config);

        inst_gen_ddr2sram(
            &_compute_inst,
            Pointcloud_online_utils::layer_config,
            cfg,
            "pt");

        inst_gen_ddr2sram(
            &_compute_inst,
            Pointcloud_online_utils::layer_config,
            cfg,
            "in");

        cnn_inst_index = _compute_inst.size(); // 4 is the number of uint32_t in each instruction
        
        // printf("cnn_inst_%d\n", cnn_inst_index);

        inst_gen_cnn(
            &_compute_inst,
            Pointcloud_online_utils::layer_config,
            cfg);

        inst_gen_sram2ddr(
            &_compute_inst,
            Pointcloud_online_utils::layer_config,
            cfg);

        inst_gen_finish(&_compute_inst);

        memcpy(const_cast<uint32_t*>(dma_inst), _compute_inst.data(), _compute_inst.size() * sizeof(uint32_t));

        return cnn_inst_index; // Return the index of the CNN instruction in the compute instruction
    }

    // Compute each block with the instruction generated only one time
    void Pointcloud_online_utils::run_block(int cnn_inst_index){
        // Timer timer_in_cpy,timer_run, timer_rs_cpy;
        // timer_in_cpy.reset();
        // timer_run.reset();
        // timer_rs_cpy.reset();

        uint64_t pt_offset = (block_config[0].BASE_PT_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) / 8;
        uint64_t in_offset = (block_config[0].BASE_IN_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) / 8;
        uint64_t rs_offset = (block_config[0].BASE_RS_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) / 8;

        // Write the Point and Feature data to DDR
        for (int i = 0; i < int(Pointcloud_online_utils::block_config.size()); i++){
            // Copy the instuction and point data to DDR
            // timer_in_cpy.start();

            dma_inst[cnn_inst_index] = ((0x3F & Pointcloud_online_utils::layer_config.cfg_start_pt_addr)<<26) | (block_config[i].cfg_total_point_num<<11) | block_config[i].cfg_cam_valid_num;
            memcpy(const_cast<uint64_t*>(ddrbuf) + pt_offset, block_config[i].pt_data.data(), block_config[i].cfg_cam_valid_num * sizeof(uint64_t));
            memcpy(const_cast<uint64_t*>(ddrbuf) + in_offset, block_config[i].in_data.data(), block_config[i].cfg_cam_valid_num * (Pointcloud_online_utils::layer_config.cfg_inch_loop_num+1) * sizeof(uint64_t));

            // timer_in_cpy.stop();

            // Run 
            // timer_run.start();
            apb_regs->CFG_FINISH_CLR = 1;
            apb_regs->CFG_FINISH_CLR = 0;
            apb_regs->TOP_START = 0;
            apb_regs->TOP_START = 1;
            apb_regs->TOP_START = 0;

            while(apb_regs->CFG_TOP_FINISH == 0);


            apb_regs->CFG_FINISH_CLR = 1;
            apb_regs->CFG_FINISH_CLR = 0;
            // timer_run.stop();

            // RS Read
            // timer_rs_cpy.start();

            int used_core_num = get_used_core_num(Pointcloud_online_utils::layer_config.cfg_core_enable);
            Pointcloud_online_utils::block_config[i].rs_data.resize(block_config[i].cfg_cam_valid_num * used_core_num);
            for (int core = 0; core < used_core_num; core++){
                // assert( (block_config[i].BASE_RS_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR + (0x2000 * core) ) % 8 == 0);
                uint64_t* target_ptr = Pointcloud_online_utils::block_config[i].rs_data.data() + core * block_config[i].cfg_cam_valid_num;
                memcpy(target_ptr, const_cast<uint64_t*>(ddrbuf) + rs_offset + (0x2000 * core) / 8, block_config[i].cfg_cam_valid_num * sizeof(uint64_t)) ;
                // #ifdef DEBUG
                //     char _save_path[512];
                //     sprintf(_save_path, "%s/result_block_%06d_pointnum_%07d_core_%d_layer_%d.bin", Pointcloud_online_utils::save_folder, i, block_config[i].cfg_cam_valid_num, core, Pointcloud_online_utils::layer_config.layer_id);
                //     check_folder(Pointcloud_online_utils::save_folder);
                //     write_bin_file(_save_path, target_ptr, Pointcloud_online_utils::block_config[i].cfg_cam_valid_num);
                // #endif
            }
            // timer_rs_cpy.stop();
        }

        // timer_in_cpy.print( "in data copy time");
        // timer_run.print( "run time");
        // timer_rs_cpy.print( "rs data copy time");
    }    

    void Pointcloud_online_utils::run_block()
    {
        Timer timer_inst_cpy, timer_in_cpy, timer_run, timer_rs_cpy;
        timer_inst_cpy.reset();
        timer_in_cpy.reset();
        timer_run.reset();
        timer_rs_cpy.reset();
        Timer timer_inst_gen, timer_inst_real_cpy;
        timer_inst_gen.reset();
        timer_inst_real_cpy.reset();
        std::vector<uint32_t> _compute_inst;
        // Write the Point and Feature data to DDR
        for (int i = 0; i < int(Pointcloud_online_utils::block_config.size()); i++)
        // for (int i = 0; i < 1; i++)
        {
            timer_inst_cpy.start();
            timer_inst_gen.start();
            BlockConfig cfg = Pointcloud_online_utils::block_config[i];
            if (BlockConfigCache::inst_map.find(cfg.cfg_cam_valid_num) != BlockConfigCache::inst_map.end())
            {
                // If the block size is already cached, we can reuse the instruction
                _compute_inst = BlockConfigCache::inst_map[cfg.cfg_cam_valid_num];
            }
            else
            {
                _compute_inst.clear();

                inst_gen_param(
                    &_compute_inst,
                    Pointcloud_online_utils::layer_config);

                inst_gen_ddr2sram(
                    &_compute_inst,
                    Pointcloud_online_utils::layer_config,
                    cfg,
                    "pt");

                inst_gen_ddr2sram(
                    &_compute_inst,
                    Pointcloud_online_utils::layer_config,
                    cfg,
                    "in");

                inst_gen_cnn(
                    &_compute_inst,
                    Pointcloud_online_utils::layer_config,
                    cfg);

                inst_gen_sram2ddr(
                    &_compute_inst,
                    Pointcloud_online_utils::layer_config,
                    cfg);

                inst_gen_finish(&_compute_inst);
                // Cache the instruction for future use
                BlockConfigCache::inst_map[cfg.cfg_cam_valid_num] = _compute_inst;
            }
            timer_inst_gen.stop();
            timer_inst_real_cpy.start();
            // Directly write inst to the SRAM
            memcpy(const_cast<uint32_t*>(dma_inst), _compute_inst.data(), _compute_inst.size() * sizeof(uint32_t));
            timer_inst_real_cpy.stop();
            _compute_inst.clear();
            timer_inst_cpy.stop();

            timer_in_cpy.start();
            BlockConfig block_cfg = Pointcloud_online_utils::block_config[i];
            // Read Point
            assert(block_cfg.pt_data.size() == size_t(block_cfg.cfg_cam_valid_num));
            assert((block_cfg.BASE_PT_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) % 8 == 0);
            uint64_t pt_offset = (block_cfg.BASE_PT_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) / 8;
            memcpy(const_cast<uint64_t*>(ddrbuf) + pt_offset, block_cfg.pt_data.data(), block_cfg.cfg_cam_valid_num * sizeof(uint64_t));

            // Read IN
            assert(block_cfg.in_data.size() == size_t(block_cfg.cfg_cam_valid_num * (Pointcloud_online_utils::layer_config.cfg_inch_loop_num + 1)));
            assert((block_cfg.BASE_IN_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) % 8 == 0);
            uint64_t in_offset = (block_cfg.BASE_IN_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) / 8;
            memcpy(const_cast<uint64_t*>(ddrbuf) + in_offset, block_cfg.in_data.data(), block_cfg.cfg_cam_valid_num * (Pointcloud_online_utils::layer_config.cfg_inch_loop_num + 1) * sizeof(uint64_t));
            timer_in_cpy.stop();
            // Run
            timer_run.start();
            apb_regs->CFG_FINISH_CLR = 1;
            apb_regs->CFG_FINISH_CLR = 0;
            apb_regs->TOP_START = 0;
            apb_regs->TOP_START = 1;
            apb_regs->TOP_START = 0;
            while (apb_regs->CFG_TOP_FINISH == 0)
                ;
            // printf("Compute SCNN successfully for block %d. \n", i);
            apb_regs->CFG_FINISH_CLR = 1;
            apb_regs->CFG_FINISH_CLR = 0;
            timer_run.stop();

            int used_core_num = get_used_core_num(Pointcloud_online_utils::layer_config.cfg_core_enable);

            timer_rs_cpy.start();
            Pointcloud_online_utils::block_config[i].rs_data.resize(block_cfg.cfg_cam_valid_num * used_core_num);
            // RS Read
            for (int core = 0; core < used_core_num; core++)
            {
                assert((block_cfg.BASE_RS_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR + (0x2000 * core)) % 8 == 0);
                uint64_t rs_offset = (block_cfg.BASE_RS_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR + (0x2000 * core)) / 8;
                uint64_t *target_ptr = Pointcloud_online_utils::block_config[i].rs_data.data() + core * block_cfg.cfg_cam_valid_num;
                memcpy(target_ptr, const_cast<uint64_t*>(ddrbuf) + rs_offset, block_cfg.cfg_cam_valid_num * sizeof(uint64_t));
// #ifdef DEBUG
//                 char _save_path[512];
//                 sprintf(_save_path, "%s/result_block_%06d_pointnum_%07d_core_%d_layer_%d.bin", Pointcloud_online_utils::save_folder, i, block_cfg.cfg_cam_valid_num, core, Pointcloud_online_utils::layer_config.layer_id);
//                 check_folder(Pointcloud_online_utils::save_folder);
//                 write_bin_file(_save_path, target_ptr, Pointcloud_online_utils::block_config[i].cfg_cam_valid_num);
//                 sprintf(_save_path, "%s/weight_layer_%d.bin", Pointcloud_online_utils::save_folder, Pointcloud_online_utils::layer_config.layer_id);
//                 write_bin_file(_save_path, layer_config.wt_data.data(), layer_config.wt_data.size());
//                 sprintf(_save_path, "%s/pt_block_%d_layer_%d.bin", Pointcloud_online_utils::save_folder, i, Pointcloud_online_utils::layer_config.layer_id);
//                 write_bin_file(_save_path, block_cfg.pt_data.data(), block_cfg.pt_data.size());
//                 sprintf(_save_path, "%s/in_block_%d_layer_%d.bin", Pointcloud_online_utils::save_folder, i, Pointcloud_online_utils::layer_config.layer_id);
//                 write_bin_file(_save_path, block_cfg.in_data.data(), block_cfg.in_data.size());
// #endif
            }
            timer_rs_cpy.stop();
        }
        // Cleanup the cache
        BlockConfigCache::inst_map.clear();

        timer_inst_cpy.print("Inst Copy Time");
        timer_in_cpy.print("Input Copy Time");
        timer_run.print("Run Time");
        timer_rs_cpy.print("RS Copy Time");
        timer_inst_gen.print("Inst Gen Time");
        timer_inst_real_cpy.print("Inst Real Copy Time");
    }

    void Pointcloud_online_utils::run()
    {
        // Before running this function, we need to make sure that wt_init and compute_init have been called.
        // Start
        apb_regs->CFG_FINISH_CLR = 1;
        apb_regs->CFG_FINISH_CLR = 0;
        apb_regs->TOP_START = 0;
        apb_regs->TOP_START = 1;
        apb_regs->TOP_START = 0;
        while (apb_regs->CFG_TOP_FINISH == 0)
            ;
        printf("Compute SCNN successfully. \n");
        apb_regs->CFG_FINISH_CLR = 1;
        apb_regs->CFG_FINISH_CLR = 0;

        int used_core_num = get_used_core_num(Pointcloud_online_utils::layer_config.cfg_core_enable);

        for (int i = 0; i < int(Pointcloud_online_utils::block_config.size()); i++)
        {
            BlockConfig block_cfg = Pointcloud_online_utils::block_config[i];
            Pointcloud_online_utils::block_config[i].rs_data.resize(block_cfg.cfg_cam_valid_num * used_core_num);
            // RS Read
            for (int core = 0; core < used_core_num; core++)
            {
                assert((block_cfg.BASE_RS_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR + (0x2000 * core)) % 8 == 0);
                uint64_t rs_offset = (block_cfg.BASE_RS_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR + (0x2000 * core)) / 8;
                // Pointcloud_online_utils::block_config[i].rs_data.clear();
                // Pointcloud_online_utils::block_config[i].rs_data.resize(block_cfg.cfg_cam_valid_num);
                // memcpy(Pointcloud_online_utils::block_config[i].rs_data.data(), ddrbuf + rs_offset, block_cfg.cfg_cam_valid_num * sizeof(uint64_t)) ;
                uint64_t *target_ptr = Pointcloud_online_utils::block_config[i].rs_data.data() + core * block_cfg.cfg_cam_valid_num;
                memcpy(target_ptr, const_cast<uint64_t*>(ddrbuf) + rs_offset, block_cfg.cfg_cam_valid_num * sizeof(uint64_t));
                // printf("Copy Len: %d\n", block_cfg.cfg_cam_valid_num * sizeof(uint64_t));
                // For DEBUG use!
                char _save_path[512];
                sprintf(_save_path, "%s/result_block_%06d_pointnum_%07d_core_%d_layer_%d.bin", Pointcloud_online_utils::save_folder, i, block_cfg.cfg_cam_valid_num, core, Pointcloud_online_utils::layer_config.layer_id);
                check_folder(Pointcloud_online_utils::save_folder);
                write_bin_file(_save_path, target_ptr, Pointcloud_online_utils::block_config[i].cfg_cam_valid_num);
            }
        }
    }
    void Pointcloud_online_utils::run(char inst_file[256], char save_folder[256], int core_num)
    {
        LayerConfig layer_cfg = Pointcloud_online_utils::layer_config;
        read_bin_file(inst_file, const_cast<uint32_t*>(dma_inst));
        // assert(ret > 0);
        for (int i = 0; i < int(Pointcloud_online_utils::block_config.size()); i++)
        {
            BlockConfig block_cfg = Pointcloud_online_utils::block_config[i];
            // Read Point
            assert(block_cfg.pt_data.size() == size_t(block_cfg.cfg_cam_valid_num));
            assert((block_cfg.BASE_PT_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) % 8 == 0);
            uint64_t pt_offset = (block_cfg.BASE_PT_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) / 8;
            memcpy(const_cast<uint64_t*>(ddrbuf) + pt_offset, block_cfg.pt_data.data(), block_cfg.cfg_cam_valid_num * sizeof(uint64_t));

            // Read IN
            assert(block_cfg.in_data.size() == size_t(block_cfg.cfg_cam_valid_num * (layer_cfg.cfg_inch_loop_num + 1)));
            assert((block_cfg.BASE_IN_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) % 8 == 0);
            uint64_t in_offset = (block_cfg.BASE_IN_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR) / 8;
            memcpy(const_cast<uint64_t*>(ddrbuf) + in_offset, block_cfg.in_data.data(), block_cfg.cfg_cam_valid_num * (layer_cfg.cfg_inch_loop_num + 1) * sizeof(uint64_t));
        }

        // Start
        apb_regs->CFG_FINISH_CLR = 1;
        apb_regs->CFG_FINISH_CLR = 0;
        apb_regs->TOP_START = 0;
        apb_regs->TOP_START = 1;
        apb_regs->TOP_START = 0;
        while (apb_regs->CFG_TOP_FINISH == 0)
            ;
        printf("Compute SCNN successfully. \n");
        apb_regs->CFG_FINISH_CLR = 1;
        apb_regs->CFG_FINISH_CLR = 0;

        for (int i = 0; i < int(Pointcloud_online_utils::block_config.size()); i++)
        {
            BlockConfig block_cfg = Pointcloud_online_utils::block_config[i];
            // RS Read
            for (int core = 0; core < core_num; core++)
            {
                assert((block_cfg.BASE_RS_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR + (0x2000 * core)) % 8 == 0);
                uint64_t rs_offset = (block_cfg.BASE_RS_ADDR - Pointcloud_online_utils::DDR_BASE_ADDR + (0x2000 * core)) / 8;
                Pointcloud_online_utils::block_config[i].rs_data.clear();
                Pointcloud_online_utils::block_config[i].rs_data.resize(block_cfg.cfg_cam_valid_num);
                memcpy(block_cfg.rs_data.data(), const_cast<uint64_t*>(ddrbuf) + rs_offset, block_cfg.cfg_cam_valid_num * sizeof(uint64_t));

                // printf("Copy Len: %d\n", block_cfg.cfg_cam_valid_num * sizeof(uint64_t));
                char _save_path[256];
                sprintf(_save_path, "%s/result_block_%06d_pointnum_%07d_core_%d.bin", save_folder, i, block_cfg.cfg_cam_valid_num, core);
                check_folder(save_folder);
                write_bin_file(_save_path, block_cfg.rs_data.data(), block_cfg.cfg_cam_valid_num);
            }
        }
    }
}
